import sensor, image, time, pyb,lcd
from pyb import UART
from pyb import LED
import ustruct
import time

red_threshold = (34, 81, 9, 78, 8, 73)
green_threshold  = (60, 75, -70, -40, 20, 60)
black_threshold = (0, 10, -3, 3, -5, 5)
yellow_threshold = (85, 100, -20, 10, 40, 70)
gray_threshold = ()

uart3 = UART(3, 115200, timeout_char=15)

dat = {
    'road':[black_threshold, 160, 1],
    'red':[red_threshold, -1, 2],
    'yellow':[yellow_threshold, -1, 2],
    'green':[green_threshold, -1, 2]
}

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=3000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
lcd.init()

def find_road(img):
    blob = img.find_blobs([black_threshold], pixels_threshold=1000, area_threshold=1000)
    if blob:
        s = 0
        for i in blob:
            if i[2]*i[3] > s:
                s = i[2]*i[3]
                blo = i
        img.draw_rectangle(blo.rect())
        img.draw_cross(blo.cx(), blo.cy())
        b = blo.cx()
    else:b=160
    return b

def find_color(img, color):
    blob = img.find_blobs([color], pixels_threshold=500, area_threshold=500)
    if blob:
        s=0
        for i in blob:
            if i[2]*i[3] > s:
                s = i[2]*i[3]
                blo = i
    else:blo=False
    return blo

def get_y(img, color):
    rs = find_color(img, color)
    if rs == False:return -1
    img.draw_rectangle(rs.rect())
    img.draw_cross(rs.cx(), rs.cy())
    return rs.cy()

def get_distance(img, color, K):
    blo = find_color(img, color)
    if blo == False:return -1
    Lm = (blo[2]+blo[3])/2
    length = K/Lm
    return length

def test_distance(img, color, cm=10):
    blo = find_color(img, color)
    if blo == False:return -1
    t = cm*blo.cx()
    return t

def get_data(uart3):
    if uart3.any():
        dat = uart3.read()
        if dat==b'\x00' or dat==None or dat == '0':return ''
        dat = ustruct.unpack('<bbbbbbbbbbbbbbbbbbbb',dat)
        print(dat)
        dat = dat[14]
        return dat

def write_data(uart3, data):
    if len(data) > 0:
        cmd = [0x56,0xAB,0x14,0xE8,0x13,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0xCF]
        for i in range(0, len(data)):
            cmd[i+5] = data[i]
        print(cmd)
        uart3.write(ustruct.pack("<bbbbbbbbbbbbbbbbbbbb",cmd[0],cmd[1],cmd[2],cmd[3],cmd[4],cmd[5],cmd[6],cmd[7],cmd[8],cmd[9],cmd[10],cmd[11],cmd[12],cmd[13],cmd[14],cmd[15],cmd[16],cmd[17],cmd[18],cmd[19]))

def read_data(uart3):
    cmd = 0
    data = 0
    if uart3.any():
        buf = bytearray(20)
        uart3.readinto(buf)
        cmd = buf[5]
        data = buf[6]
    return cmd,data

def command(uart3):
    global dat
    cmd,data = read_data(uart3)
    #if cmd == 0 or data == 0:return
    print(cmd,data)
    write_data(uart3, [dat['road'][1],dat['red'][1],dat['yellow'][1],dat['green'][1]])

while True:
    img = sensor.snapshot().lens_corr(1.8)
    for meth in dat:
        next = dat[meth][2]
        if next == 1:dat[meth][1] = find_road(img)
        elif next == 2:dat[meth][1] = get_y(img, dat[meth][0])
    print(str(dat))
    lcd.display(img)
    command(uart3)
